/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package mygame.procedural.ik;

import com.jme3.bullet.control.BetterCharacterControl;
import com.jme3.math.FastMath;
import com.jme3.math.Vector3f;
import com.jme3.scene.Node;
import com.jme3.scene.Spatial;
import mygame.procedural_walk.GaitManager;

/**
 *
 * @author Paulo
 */
public class WalkIkControl {

    private GaitManager gait;
    private Spatial model;
    private Node ground;
    private Node leftTarget;
    private Node rightTarget;
    
    private float stepHeight = 0.2f;
    private float stepLength = 0.3f;
    
    private LegControl left;
    private LegControl right;
    
    private BetterCharacterControl control;
    
    private Vector3f velocity = new Vector3f();

    public WalkIkControl(Spatial model, Node ground, Node leftTarget, Node rightTarget) {
        this.model = model;
        this.ground = ground;
        this.leftTarget = leftTarget;
        this.rightTarget = rightTarget;
        this.control = model.getControl(BetterCharacterControl.class);
        
        this.gait = new GaitManager(new float[] {0f, 0.5f}, new LegControl[] {
            leftTarget.getControl(LegControl.class),
            rightTarget.getControl(LegControl.class)
        });
        this.left = leftTarget.getControl(LegControl.class);
        this.left.setVelocity(this.velocity);
        this.right = rightTarget.getControl(LegControl.class);
        this.right.setVelocity(this.velocity);
    }
    private float time = 0;

    private void recalcVelocity() {
        velocity.set(control.getVelocity());
    }
    
    public void update(float tpf) {
        recalcVelocity();
        
        float stepTime = stepLength / Math.max(velocity.length(), 0.8f);
        
        if (time > 2 * stepTime) {
            time = 0;
        }
        time += tpf;

        if (control.isOnGround()) {
            if (FastMath.floor(time / stepTime) % 2 == 0) {
                //left.stance();
                right.swing();
            } else {
                left.swing();
                //right.stance();
            }
        } else {
            left.swing();
            right.swing();
        }
    }
}
